#include "mainwindow.h"
#include "ui_mainwindow.h"
#include <QMessageBox>
#include <QSettings>
#include <QFileInfo>
#include <QSettings>

#define AXIS_X_MAX (2500)

MainWindow::MainWindow(QWidget *parent) :
    QMainWindow(parent),
    ui(new Ui::MainWindow)
{

    this->setMinimumSize(788, 475);
    connect(this, SIGNAL(openDone(bool)), this, SLOT(onOpenDone(bool)));
    connect(this, SIGNAL(moveDone(double, bool, bool)),this, SLOT(onMoveDone(double, bool, bool)));
    connect(&moving_server, SIGNAL(sigMoveServo(int)),this, SLOT(slot_move_servo(int)));
    connect(this, SIGNAL(sigMoveServo(std::string, std::string)),&moving_server, SLOT(slotReplyClient(std::string, std::string)));

    m_servo = std::unique_ptr<servoControl>(new servoControl());
    loadInit();
    ui->setupUi(this);
}

MainWindow::~MainWindow()
{
    LOG(INFO) << "~MainWindow start";
    delete ui;
    LOG(INFO) << "~MainWindow end";
}

void MainWindow::closeEvent(QCloseEvent *event)
{
    LOG(INFO) << "closeEvent start";

    if(worker.joinable())
    {
        mWorkerProcMutex.lock();
        mWorkerRunnig = false;
        mWorkerRecvStatus = true;
        mWorkerProcMutex.unlock();

        mWorkercond.notify_all();
        worker.join();
    }
    disconnect(this, nullptr, nullptr, nullptr);
    disconnect(&moving_server, nullptr, nullptr, nullptr);
    LOG(INFO) << "closeEvent end";
}

void MainWindow::on_pushButton_8_clicked()
{
    if(ui->pushButton_8->text() == tr("启动"))
    {
        ui->pushButton_8->setEnabled(false);
        std::thread openthread = std::thread(&MainWindow::openDev, this);
        openthread.detach();
    }
}


void MainWindow::openDev()
{
    //openDev()函数如果成功，只会被执行一次
    bool ret = false;

#if 1
    if(false == mMonitorRunnig){
        LOG(INFO) << "start monitorThreadFun thread\n";
        mMonitorRunnig = true;
        monitor = std::thread(&MainWindow::monitorThreadFun, this);
    }


    if(m_servo)
    {
        int axisNum = 0;
        if(!m_servo->connectCtrler(axisNum))
        {
            LOG(ERROR) << "Servo connect Failed!!!\n";
            ret = false;
            goto ERR_END_OFF_FUNC;
        }
        else
        {
            LOG(INFO) << "Servo connect Success! Current Axis: " << axisNum << std::endl;
            if(!m_servo->enableAllAxis(true))
            {
                LOG(ERROR) << "Axis Enable Failed!!!\n";
                ret = false;
                goto ERR_END_OFF_FUNC;
            }
        }
        LOG(INFO) << "Servo enableAllAxis Success!!!\n";
    }

    if(false == mServoUiRunnig){
        LOG(INFO) << "start mServoUiRunnig thread\n";
        mServoUiRunnig = true;
        servoUi = std::thread(&MainWindow::servoUiThreadFun, this);
    }
#endif

#if 1
    //打开server
    ret = moving_server.init();
    if(!ret){
        LOG(ERROR) << "Moving server init err!" << std::endl;
    }
#endif


    if(false == mWorkerRunnig){
        LOG(INFO) << "start mWorkerRunnig thread\n";
        mWorkerRunnig = true;
        worker = std::thread(&MainWindow::workThreadFun, this);
    }


ERR_END_OFF_FUNC:
    emit openDone(ret);
}

void MainWindow::onOpenDone(bool ret)
{
    if(ret)
    {
        ui->pushButton_8->setText("已启动");
    }else{
        ui->pushButton_8->setEnabled(true);
    }
}

void MainWindow::onMoveDone(double x, bool result, bool popup)
{
    float valueX = x;
    if(ui->isMM->isChecked())
    {
        valueX = Pulse2mmForX(valueX);
    }

    ui->editCurPosX->setText(QString::number(valueX));
    if(popup){
        if(!result)
        {
            QMessageBox::critical(this, tr("警告"),
                                  tr("当前操作失败"),
                                  QMessageBox::Ok,
                                  QMessageBox::Ok);
        }
        else
        {
            QMessageBox::information(this, tr("提示"),
                                     tr("当前操作成功"),
                                     QMessageBox::Ok,
                                     QMessageBox::Ok);
        }
    }
}

void MainWindow:: monitorThreadFun()
{
    LOG(INFO) << "monitor thread "<< std::this_thread::get_id() <<" has start.\n";
    int time =0;
    int servo_reset =0;
    int last_mMonitorEmergencySwitch = mMonitorEmergencySwitch;
    while(mMonitorRunnig){
        int nRet = m_servo->pingger.get_ip_status();
        if(PING_ERR ==  nRet ){
            LOG(ERROR) << "m_servo pinnger err" << std::endl;
            ui->radioButton->setChecked(false);
            std::this_thread::sleep_for(std::chrono::milliseconds(500));
        }else{
            ui->radioButton->setChecked(true);
            if(m_servo->getConnectStatus()){
                m_servo->get_input_pin(EMERGENCY_STOP_IN_PIN, mMonitorEmergencySwitch);
                //LOG(ERROR) << "monitor_thread_fun mMonitorEmergencySwitch ="<<mMonitorEmergencySwitch<<"\n";
                if(EMERGENCY_BUTTON_PRESS_DOWN == mMonitorEmergencySwitch){
                    std::this_thread::sleep_for(std::chrono::milliseconds(500));
                    m_servo->get_input_pin(EMERGENCY_STOP_IN_PIN, mMonitorEmergencySwitch);
                    if(EMERGENCY_BUTTON_PRESS_DOWN == mMonitorEmergencySwitch){
                        std::this_thread::sleep_for(std::chrono::milliseconds(500));
                        if(last_mMonitorEmergencySwitch != mMonitorEmergencySwitch){
                            //关闭程序执行
                            LOG(ERROR) << "Emergency Switch Press Down" << std::endl;
                            last_mMonitorEmergencySwitch = mMonitorEmergencySwitch;
                        }
                    }
                }else{
                    if(last_mMonitorEmergencySwitch != mMonitorEmergencySwitch){
                        LOG(INFO) << "Emergency Switch Lift On" << std::endl;
                        LOG(INFO) << "Reconnect Devices..." << std::endl;
                        last_mMonitorEmergencySwitch = mMonitorEmergencySwitch;
                        servo_reset = 1;
                    }
                    std::this_thread::sleep_for(std::chrono::milliseconds(500));
                }
            }
        }

        //重新连接servo
        if(servo_reset == 1){
            if(-1 == reconnect_servo()){
                LOG(ERROR) << "reconnect servo err！" << std::endl;
            }else{
                LOG(INFO) <<  "reconnect servo success！" << std::endl;
                servo_reset = 0;
            }
        }

        if(time++%20 == 0){
            LOG(INFO) << "monitorThreadFun time " << time <<"\n";
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(500));
    }
}

int MainWindow::reconnect_servo()
{
    LOG(INFO) << "Servo reconnect ... "<< std::endl;
    m_servo = std::unique_ptr<servoControl>(new servoControl());
    if(!m_servo){
        LOG(ERROR) << "new servoControl() err! \n";
        return -1;
    }

    int axisNum = 0;
    if(!m_servo->connectCtrler(axisNum)){
        LOG(ERROR) << "Servo connect Failed!!!\n";
        return -1;
    }else{
        LOG(INFO) << "Servo connect Success! Current Axis: " << axisNum << std::endl;
        if(!m_servo->enableAllAxis(true)){
            return -1;
            LOG(ERROR) << "Axis Enable Failed!!!\n";
        }
    }
    LOG(INFO) << "Servo reconnect Success!!!\n";
    return 0;
}

void MainWindow::servoUiThreadFun()
{
    while(mServoUiRunnig){
        if(m_servo->getConnectStatus()){
            servo_update_ui();
            std::this_thread::sleep_for(std::chrono::milliseconds(200));
        }else{
            std::this_thread::sleep_for(std::chrono::milliseconds(500));
        }
    }
}

void MainWindow::servo_update_ui()
{
    //更新UI显示的电机位置
    double x = 0.0;
    servo_get_cur_posxy(x);
    emit moveDone(x, true, false);
}


void MainWindow::servo_get_cur_posxy(double &x)
{
    double posX = 0.0;
    if(m_servo)
    {
        m_servo->getPosition(AXIS_X, posX);
    }
    x = posX;
    //LOG(INFO) << "posX = " << posX;
}

/////
/// \brief MainWindow::servo_move_axis:移动行架，并更新显示
/// \param axis_num：轴编号
/// \param dist_mm：移动距离，单位mm
/// \return 成功true，失败false
///
bool MainWindow::servo_move_axis(int axis_num,int dist_mm,int& opt_errno)
{
    LOG(INFO) << "axis_num  " << axis_num << " move dist_mm "  << dist_mm << "!"<<std::endl;
    double dist_pulse;
    if(axis_num == AXIS_X){
        if(dist_mm>0){
            dist_mm =0;
        }
        if(dist_mm<-AXIS_X_MAX){
            dist_mm = -AXIS_X_MAX;
        }
        dist_pulse = mm2PulseForX(dist_mm);
    }else{
        LOG(ERROR) << "axis_num cann't be  " << axis_num <<std::endl;
        assert(false);
    }

    if(!m_servo->runSingleAxis(axis_num, dist_pulse, MOVE_ABSOLUTE)){
        LOG(ERROR) << "Servo axis_num " <<  axis_num << "dist_mm "  << dist_mm << "move failed!"<<std::endl;
        opt_errno =-OPT_SERVO_ERR;
        return false;
    }
    servo_update_ui();
    return true;
}

void MainWindow::on_pushButton_3_clicked()
{
    //x轴位置每次减少10mm
    //    double posX;
    //    m_servo->getPosition(AXIS_X, posX);
    //    int cur_xmm = Pulse2mmForX(posX);
    //    cur_xmm -= 1;
    //    LOG(INFO) << "cur_xmm :" << cur_xmm << std::endl;
    //    servo_move_axis(AXIS_X, cur_xmm, opt_errno);

    //x轴位置每次减少10mm
    double posX=0;
    m_servo->getPosition(AXIS_X, posX);
    posX -= 1000;
    if(!m_servo->runSingleAxis(AXIS_X, posX, MOVE_ABSOLUTE)){
        LOG(ERROR) << "Servo axis_num " <<  AXIS_X << "posX "  << posX << "move failed!"<<std::endl;
        opt_errno =-OPT_SERVO_ERR;
    }
    LOG(INFO) << "posX-- end";
}

void MainWindow::on_pushButton_clicked()
{

    //    int cur_xmm = Pulse2mmForX(posX);
    //    cur_xmm += 1;
    //    LOG(INFO) << "cur_xmm :" << cur_xmm << std::endl;
    //    servo_move_axis(AXIS_X, cur_xmm, opt_errno);
    //x轴位置每次增加10mm
    double posX=0;
    m_servo->getPosition(AXIS_X, posX);
    posX += 1000;
    if(!m_servo->runSingleAxis(AXIS_X, posX, MOVE_ABSOLUTE)){
        LOG(ERROR) << "Servo axis_num " <<  AXIS_X << "posX "  << posX << "move failed!"<<std::endl;
        opt_errno =-OPT_SERVO_ERR;
    }
    LOG(INFO) << "posX++ end";
}

void MainWindow::on_pushButton_2_clicked()
{
    LOG(INFO) << "ZERO_AXIS_X start";
    mWorkerProcMutex.lock();
    mWorkerCurTask = ZERO_AXIS_X;
    mWorkerRecvStatus = true;
    mWorkerProcMutex.unlock();

    mWorkercond.notify_all();
    LOG(INFO) << "ZERO_AXIS_X end";
}

void MainWindow::workThreadFun()
{
    LOG(INFO) << "work thread "<< std::this_thread::get_id() <<" has start.\n";
    bool bChargeSuccess = false;
    while(mWorkerRunnig)
    {
        std::unique_lock<std::mutex> lk(mWorkerProcMutex);
        mWorkercond.wait(lk, [this]{return mWorkerRecvStatus;});
        mWorkerRecvStatus = false;
        if(!mWorkerRunnig) {
            lk.unlock();
            break;
        }

        switch (mWorkerCurTask) {
        case ZERO_AXIS_X:
        {
            LOG(INFO) << "start ZERO_AXIS_X ";
            bool ret = m_servo->runBackZero(AXIS_X);
            double x = 0.0;
            servo_get_cur_posxy(x);
            emit moveDone(x, ret, true);
            mWorkerCurTask = TASK_WAIT;
            break;
        }

        case MOVE_AXIS_X:
        {
            bool ret = true;
            if(ui->editDisX->text() != ""){
                float dis = ui->editDisX->text().toFloat();

                if(dis<=0 && dis>=-AXIS_X_MAX){
                    if(ui->isMM->isChecked()){
                        dis = mm2PulseForX(dis);
                    }
                    ret = m_servo->runSingleAxis(AXIS_X, dis, MOVE_ABSOLUTE);
                }
                double x = 0.0;
                servo_get_cur_posxy(x);
                emit moveDone(x,ret, true);
                mWorkerCurTask = TASK_WAIT;
                break;
            }
        }

        case MOVE_AXIS_X_NUM:
        {
            bool ret = true;
            std::string reslutStr;
            std::string numStr = std::to_string(cur_point_num);
            if(cur_point_num>3||cur_point_num<1){
                reslutStr = "Failed";
            }else{
                int dis_pluse =  mm2PulseForX(pointDis[cur_point_num]);
                ret = m_servo->runSingleAxis(AXIS_X, dis_pluse, MOVE_ABSOLUTE);
                if(ret){
                    reslutStr = "Passed";
                }else{
                    reslutStr = "Failed";
                }
            }
            emit sigReplyClient(numStr, reslutStr);

            double x = 0.0;
            servo_get_cur_posxy(x);
            emit moveDone(x,ret, true);
            mWorkerCurTask = TASK_WAIT;
            break;
        }

        case MOVE_HOME:
        {
            bool ret = true;
            double x = 0.0;
            ret = servo_reset_home();
            mWorkerCurTask = TASK_WAIT;
            servo_get_cur_posxy(x);
            emit moveDone(x, ret, true);
            mWorkerCurTask = TASK_WAIT;
            break;
        }
        case ENABLE:
        {
            // 电机轴使能
            bool ret = m_servo->enableAllAxis(true);
            double x= 0.0;
            servo_get_cur_posxy(x);
            emit moveDone(x, ret, true);
            mWorkerCurTask = TASK_WAIT;
            break;
        }
        case DISENABLE:
        {
            // 电机轴下使能
            bool ret = m_servo->enableAllAxis(false);
            double x = 0.0;
            servo_get_cur_posxy(x);
            emit moveDone(x, ret, true);
            mWorkerCurTask = TASK_WAIT;
            break;
        }
        case TASK_WAIT:
        default:
            break;
        }
        LOG(INFO) << "Task done!Waitting for next task..." << std::endl;
    }

    LOG(INFO) << "work thread "<< std::this_thread::get_id() <<" has exit.\n";
}


bool MainWindow::servo_reset_home()
{
    bool ret = m_servo->runMoveHome();
    double x = 0.0;
    servo_get_cur_posxy(x);
    emit moveDone(x, ret, false);
    return ret;
}

void MainWindow::on_pushButton_5_clicked()
{
    mWorkerProcMutex.lock();
    mWorkerCurTask = MOVE_AXIS_X;
    mWorkerRecvStatus = true;
    mWorkerProcMutex.unlock();

    mWorkercond.notify_all();
}

void MainWindow::on_pushButton_4_clicked()
{
    m_servo->stopMove();
}

void MainWindow::on_pushButton_6_clicked()
{
    save_point_file("/SERVO/POINT1");
}

void MainWindow::on_pushButton_7_clicked()
{
    save_point_file("/SERVO/POINT2");
}

void MainWindow::on_pushButton_9_clicked()
{
    save_point_file("/SERVO/POINT3");
}

void MainWindow::on_pushButton_10_clicked()
{
    save_point_file("/SERVO/POINT4");
}

void MainWindow::slot_move_servo(int num)
{

    mWorkerProcMutex.lock();
    mWorkerCurTask = MOVE_AXIS_X_NUM;
    mWorkerRecvStatus = true;
    cur_point_num = num;
    mWorkerProcMutex.unlock();

    mWorkercond.notify_all();
}

void MainWindow::save_point_file(std::string key)
{
    //获取当前点位
    double x = 0.0;
    servo_get_cur_posxy(x);

    //写入配置文件
    QFileInfo fileInfo(CONFIG_FILE_PATH);
    if(fileInfo.isFile())
    {
        QSettings setting(CONFIG_FILE_PATH, QSettings::IniFormat);
        setting.setIniCodec("utf-8");
        setting.setValue(key.c_str(), std::to_string(x).c_str());
    }
}

void MainWindow::loadInit()
{
    QFileInfo fileInfo(CONFIG_FILE_PATH);
    if(fileInfo.isFile())
    {
        LOG(INFO) << "Using config file !"<< std::endl;
        QSettings setting(CONFIG_FILE_PATH, QSettings::IniFormat);
        setting.setIniCodec("utf-8");
        pointDis[0] = setting.value("/SERVO/POINT1").toDouble();
        pointDis[1] = setting.value("/SERVO/POINT2").toDouble();
        pointDis[2] = setting.value("/SERVO/POINT3").toDouble();
        pointDis[3] = setting.value("/SERVO/POINT4").toDouble();

        LOG(INFO) << " pointDis[0] ="<< pointDis[0] <<std::endl;
        LOG(INFO) << " pointDis[1] ="<< pointDis[1] <<std::endl;
        LOG(INFO) << " pointDis[2] ="<< pointDis[2] <<std::endl;
        LOG(INFO) << " pointDis[3] ="<< pointDis[3] <<std::endl;
    }else{
        LOG(INFO) << "Using default config !"<< std::endl;
    }
}
